package org.rlcommunity.critter;

/**
  * ObjectStateWheel
  *
  * Defines what constitutes the state of a wheel:
  *  The state is just defined as the angular velocity and the torque.
  *
  * @author csaba
  */

//import org.rlcommunity.critter.Drops.CritterControlDrop;

import java.awt.Graphics;
//import java.awt.Color;

public class ObjectStateWheel implements ObjectState
{
  /** Determines how much traction "unit" is generated proportional to the slippage [dimless] */  
  protected static final double aSlipToForceCoeff = 0.00017/0.06/5*100.0; // dimensionless
  /** The maximum traction "unit" on this wheel (at the nominal load) [dimless]
   *   In reality, this depends on the tire and the load (and the angular velocity, etc.)
   */
  protected static final double aMaxForce = 2; // 0.2G is a large force :); otherwise dimensionless
  protected static final double forcePerSpeed = 2.5;
  /** Angular velocity [rad/sec] */
  protected double aAngVel;
  /** Torque on the shaft [N m] */
  protected double aTorque;

  /** The gear ratio from the motor to the wheel [dimensionless]
   *   The actual value is that measured for NXT, 
   *   see http://www.philohome.com/nxtmotor/nxtmotor.htm
   */
  protected double aGearRatioFromMotorToWheel = 1.0; 
  /** Force lost due to friction in the gears [dimensionless] */
  protected double aTransMissionEfficiency = 1.0;
  /** The radius of the wheel [m] */
  protected double aRadius = 5E-2; // 5cm
  /** The actual load on this wheel [N]. 
   * Note that this includes the wheel's mass and more. 
   */
  protected double aLoad = 3000E-2;  
  protected double aRatedLoad = 3000E-2; // [N]; rated vertical load force (F_{z0})
  /** Maximum longitudinal force (F_x) the tire exerts on the wheel when the vertical load
   *  equals its rated value (F_{z0}).
   */
  protected double aPeakLongForceRated = 3500E-2; // [N], maximum longitudinal
  /** The value of contact slip ratio (0<=slip<=1) when F_x equals its maximum value and F_z equals
   * its rated value.
   */
  protected double aPeakSlipRatio = 0.1; // dimensionless
  /** The mass of this wheel [kg] */
  protected double aWheelMass = 20E-3; // 20g
  /** The moment inertia of this wheel [kg m^2] (computed) */
  protected double aMomentInertia;
  /** Difference of wheel and contact point slip, longitudinal direction */
  protected double aDeformationLong;
  /** Difference of wheel and contact point slip, lateral direction */
  protected double aDeformationLat;
  /** Speed limit below which damping is applied */
  protected double aLowSpeedLimit = 10E-2; // 10 cm/sec
  /** Relaxation length: 
   *   determines transient response of the tire force at the tire's rated load.
   */
  protected double aRelaxationLength = 0.2E-2; // 0.2 cm; 
  /** How much force is generated by the fire at a unit slip [N] */
  protected double aForcePerSlip  = aPeakLongForceRated/aPeakSlipRatio;
  /** Tire longitudinal stiffness [N/m] */
  protected double aTireLongStiffness = aForcePerSlip/aRelaxationLength;

public ObjectStateWheel()
  {
    aAngVel = aTorque = aDeformationLong = aDeformationLat = 0;
    computeMomentInertia();
  }

  protected void computeForceToSlipRatio()
  {
	  aForcePerSlip  = aPeakLongForceRated/aPeakSlipRatio;
  }

  /** Returns the angular velocity for this motor
    *
    * @return Angular velocity
    */
  public double getAngVelocity() { return aAngVel; }

  /** Sets the angular velocity for this motor
    * @param pAngVel New angular velocity
    */
  public void setAngVelocity(double pAngVel) { aAngVel = pAngVel; }


  /** ObjectState interface */
  public String getName()
  {
    return SimulatorComponentWheel.NAME;
  }

  public Object clone()
  {
    ObjectStateWheel newDrive = new ObjectStateWheel();
    newDrive.copyFrom(this);
    
    return newDrive;
  }

  protected void copyFrom(ObjectStateWheel org)
  {
    this.aAngVel = org.aAngVel;
    this.aTorque = org.aTorque;
    this.aGearRatioFromMotorToWheel = org.aGearRatioFromMotorToWheel; 
    this.aTransMissionEfficiency = org.aTransMissionEfficiency;
    this.aRadius = org.aRadius;
    this.aLoad = org.aLoad;
    this.aWheelMass = org.aWheelMass;
    this.aMomentInertia = org.aMomentInertia;
    this.aDeformationLong = org.aDeformationLong;
    this.aDeformationLat = org.aDeformationLat;
    this.aForcePerSlip = org.aForcePerSlip;
    this.aLowSpeedLimit = org.aLowSpeedLimit;
    this.aPeakLongForceRated = org.aPeakLongForceRated;
    this.aPeakSlipRatio = org.aPeakSlipRatio;
    this.aRatedLoad = org.aRatedLoad;
    this.aRelaxationLength = org.aRelaxationLength;
    this.aTireLongStiffness = org.aTireLongStiffness;
  }

  public void draw(Graphics g, SimulatorObject parent)
  {
      // protected Vector2D aVel;
//      Vector2D objPos = parent.getPosition();
//      double objDir = parent.getDirection();
//
//      if (aVel.x != 0)
//      {
//	      Color tempC = g.getColor();
//	      g.setColor(Color.red);
//	      // We're only drawing the x component here!
//	      int endX = (int)(objPos.x + aVel.x * Math.cos(objDir));
//	      int endY = (int)(objPos.y + aVel.x * Math.sin(objDir));
//
//	      g.drawLine((int)objPos.x, (int)objPos.y, endX, endY);
//	      g.setColor(tempC);
//      }
  }

  public void clear() {
	  assert(false); // should not be called
  }

  public double getGearRatioFromMotorToWheel() {
	  return aGearRatioFromMotorToWheel;
  }

  public void setGearRatioFromMotorToWheel(double gearRatioFromMotorToWheel) {
	  aGearRatioFromMotorToWheel = gearRatioFromMotorToWheel;
  }

  public double getTransMissionEfficiency() {
	  return aTransMissionEfficiency;
  }

  public void setTransMissionEfficiency(double transMissionEfficiency) {
	  aTransMissionEfficiency = transMissionEfficiency;
  }

  public double getRadius() {
	  return aRadius;
  }
  public void setRadius( double radius ) {
     aRadius = radius;
     computeMomentInertia();
  }

  public void setTorque(double torque) {
	  aTorque = torque;
  }

  public double getLoad() {
	  return aLoad;
  }
  
  public void setLoad( double load ) {
	  aLoad = load;
  }

  public double getSlipToForceCoeff() {
	  return aSlipToForceCoeff;
  }

  public double getMaxForce() {
	  return aMaxForce;
  }
  
  public void setWheelMass( double wheelMass ) {
	  aWheelMass = wheelMass;
	  computeMomentInertia();
  }
  public double getWheelMass() {
	  return aWheelMass;
  }
  protected void computeMomentInertia() {
	  aMomentInertia = aWheelMass * aRadius * aRadius / 2;
  }

  public double getMomentInertia() {
	  return aMomentInertia;
  }
  
  public void resetState() {
	  aAngVel = 0;
	  // we want to clear the deformation
	  aDeformationLong = 0;
	  aDeformationLat = 0;
  }

  public double getDeformationLong() {
	  return aDeformationLong;
  }

  public void setDeformationLong( double deformation ) {
	  aDeformationLong = deformation;
  }

  public double getDeformationLat() {
	  return aDeformationLat;
  }

  public void setDeformationLat( double deformation ) {
	  aDeformationLat = deformation;
  }

  public double getLowSpeedLimit() {
	  return aLowSpeedLimit;
  }

  public double getForcePerSpeed() {
	return forcePerSpeed; // tire force is generated in proportion to speed for low speeds; this gives the proportional factor
}

public double getRelaxationLength() {
	return aRelaxationLength;
}

public double getForcePerSlip() {
	return aForcePerSlip;
}

public double getTireLongStiffness() {
	return aTireLongStiffness;
}

public double getRatedLoad() {
	return aRatedLoad;
}
  
}
